#include "camera.hpp"

#include "Eigen/Dense"
#include "armor.hpp"
#include "cv_bridge/cv_bridge.h"
#include "geometry_msgs/msg/point_stamped.hpp"
#include "opencv2/opencv.hpp"
#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/camera_info.hpp"
#include "sensor_msgs/msg/image.hpp"
#include "tf2_ros/buffer.h"
#include "tf2_ros/transform_listener.h"

CameraNode::CameraNode() : Node("camera_node") {
    // clang-format off
    cameraInfo_.header.frame_id = "camera";
    cameraInfo_.k = K;
    cameraInfo_.width = width;
    cameraInfo_.height = height;
    // clang-format on

    tfBuffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
    tfListener_ = std::make_shared<tf2_ros::TransformListener>(*tfBuffer_);

    cameraFrameBroadcaster_ =
        std::make_shared<tf2_ros::TransformBroadcaster>(this);

    cameraFrameBroadcasterTimer_ =
        this->create_wall_timer(std::chrono::milliseconds(10), [this]() {
            auto camera = geometry_msgs::msg::TransformStamped();
            camera.header.stamp = this->now();
            camera.header.frame_id = "map";
            camera.child_frame_id = "camera";
            camera.transform.translation.x = 0;
            camera.transform.translation.y = 0;
            camera.transform.translation.z = 0;
            tf2::Quaternion rotation;            // x宽度，y高度，z光轴
            rotation.setRPY(M_PI_2, 0, M_PI_2);  // 需要和枪管方向同步，待改
            camera.transform.rotation.x = rotation.x();
            camera.transform.rotation.y = rotation.y();
            camera.transform.rotation.z = rotation.z();
            camera.transform.rotation.w = rotation.w();
            cameraFrameBroadcaster_->sendTransform(camera);
        });

    cameraTimer_ =
        this->create_wall_timer(std::chrono::milliseconds(10), [this]() {
            std::vector<geometry_msgs::msg::TransformStamped> armorPos(4);
            std::vector<double> armorDistSq(4);
            int k1 = 0, k2 = 1;  // k1 是最近装甲板序号，k2 是次近装甲板序号
            auto distanceSquared = [](double x1, double y1, double z1,
                                      double x2, double y2, double z2) {
                return (x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2) +
                       (z1 - z2) * (z1 - z2);
            };

            for (int i = 0; i < 4; ++i) {
                try {
                    armorPos[i] = tfBuffer_->lookupTransform(
                        "camera", "armor" + std::to_string(i),
                        tf2::TimePointZero);
                    // RCLCPP_INFO(this->get_logger(),
                    //             "armor %d, x: %lf, y: %lf, z: %lf", i,
                    //             armorPos[i].transform.translation.x,
                    //             armorPos[i].transform.translation.y,
                    //             armorPos[i].transform.translation.z);
                    armorDistSq[i] = distanceSquared(
                        armorPos[i].transform.translation.x,
                        armorPos[i].transform.translation.y,
                        armorPos[i].transform.translation.z, 0, 0,
                        0);  // camera 待定坐标在原点，看后续情况记得更改
                } catch (tf2::TransformException& ex) {
                    RCLCPP_ERROR(this->get_logger(), "%s", ex.what());
                }
            }

            for (int i = 1; i < 4; ++i) {
                if (armorDistSq[i] < armorDistSq[k1]) {
                    k2 = k1;
                    k1 = i;
                } else if (armorDistSq[i] < armorDistSq[k2]) {
                    k2 = i;
                }
            }
            // RCLCPP_INFO(this->get_logger(), "k1: %d, k2: %d", k1, k2);

            auto solve = [this](geometry_msgs::msg::TransformStamped armorPos) {
                std::vector<Eigen::Vector3d>
                    corner3d;  // 三维空间下的四个角点坐标
                std::vector<Eigen::Vector2d>
                    corner2d;  // 二维图像下的四个角点坐标

                double cx = armorPos.transform.translation.x,
                       cy = armorPos.transform.translation.y,
                       cz = armorPos.transform.translation.z;  // 装甲板中心坐标

                std::vector<Eigen::Vector3d> points = {
                    {armorLength / 2, armorWidth / 2, armorHeight / 2},
                    {armorLength / 2, -armorWidth / 2, armorHeight / 2},
                    {-armorLength / 2, -armorWidth / 2, armorHeight / 2},
                    {-armorLength / 2, armorWidth / 2, armorHeight / 2}};

                Eigen::Matrix3d R =
                    Eigen::Quaterniond(armorPos.transform.rotation.w,
                                       armorPos.transform.rotation.x,
                                       armorPos.transform.rotation.y,
                                       armorPos.transform.rotation.z)
                        .toRotationMatrix();

                // 获得了装甲板四个角点的坐标
                for (const auto& point : points)
                    corner3d.push_back(R * point + Eigen::Vector3d(cx, cy, cz));

                // RCLCPP_INFO(this->get_logger(), "center: %lf, %lf, %lf", cx,
                // cy,
                //             cz);
                // RCLCPP_INFO(this->get_logger(), "corner0: %lf, %lf, %lf",
                //             corner3d[0].x(), corner3d[0].y(),
                //             corner3d[0].z());
                // RCLCPP_INFO(this->get_logger(), "corner1: %lf, %lf, %lf",
                //             corner3d[1].x(), corner3d[1].y(),
                //             corner3d[1].z());

                // 获得装甲板四个角点在图像上的坐标
                for (int i = 0; i < 4; ++i)
                    corner2d.push_back(Eigen::Vector2d(
                        cameraInfo_.k[0] * corner3d[i].x() / corner3d[i].z() +
                            cameraInfo_.k[2],
                        cameraInfo_.k[4] * corner3d[i].y() / corner3d[i].z() +
                            cameraInfo_.k[5]));

                return corner2d;
            };

            // 最终得到的两块装甲板在图像上的二维坐标
            armorPoint[0] = solve(armorPos[k1]);
            armorPoint[1] = solve(armorPos[k2]);
            RCLCPP_INFO(this->get_logger(), "armor0: %lf, %lf, %lf, %lf",
                        armorPoint[0][0].x(), armorPoint[0][0].y(),
                        armorPoint[0][1].x(), armorPoint[0][1].y());
        });

    imagePublisher_ =
        this->create_publisher<sensor_msgs::msg::Image>("camera_image", 10);

    imagePublisherTimer_ =
        this->create_wall_timer(std::chrono::milliseconds(10), [this]() {
            cv::Mat image = cv::Mat::zeros(720, 1280, CV_8UC3);
            for (int i = 0; i < 2; ++i) {
                std::vector<cv::Point> points = {
                    cv::Point(armorPoint[i][0].x(), armorPoint[i][0].y()),
                    cv::Point(armorPoint[i][1].x(), armorPoint[i][1].y()),
                    cv::Point(armorPoint[i][2].x(), armorPoint[i][2].y()),
                    cv::Point(armorPoint[i][3].x(), armorPoint[i][3].y())};
                
                cv::fillPoly(image, points, cv::Scalar(0, 0, 255));

                cv::circle(
                    image,
                    cv::Point(armorPoint[i][0].x(), armorPoint[i][0].y()), 5,
                    cv::Scalar(0, 255, 0), -1);
                cv::circle(
                    image,
                    cv::Point(armorPoint[i][1].x(), armorPoint[i][1].y()), 5,
                    cv::Scalar(0, 255, 0), -1);
                cv::circle(
                    image,
                    cv::Point(armorPoint[i][2].x(), armorPoint[i][2].y()), 5,
                    cv::Scalar(0, 255, 0), -1);
                cv::circle(
                    image,
                    cv::Point(armorPoint[i][3].x(), armorPoint[i][3].y()), 5,
                    cv::Scalar(0, 255, 0), -1);
            }

            cv::circle(image, cv::Point(640, 360), 5, cv::Scalar(0, 0, 255),
                       -1);  // 准星
            cv::flip(image, image, -1);

            auto msg =
                cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", image)
                    .toImageMsg();
            imagePublisher_->publish(*msg);
        });
}

int main(int argc, char** argv) {
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<CameraNode>());
    rclcpp::shutdown();
    return 0;
}